#pragma once

#include <gtl/vec2.hpp>
#include <list>

namespace SmsAlg
{

    /**
    * This class interpolates a function using line segments
    */
    template<class T>
    class LinFitting
    {
        static bool PointComparer (const gtl::Vec2<T> p1, const gtl::Vec2<T> p2) 
        {
            return p1.x() < p2.x();
        }
    public:
        void SetBounds(T minX, T maxX)
        {
            if (minX > maxX)
                throw std::exception("Min x must be greater than max x. ");
            _minX = minX;
            _maxX = maxX;
        }

        /**
        * Adds an exact point of the function. 
        * Points can be added in no particular order. 
        */
        void AddPoint(gtl::Vec2<T> p){
            for (int i = 0; i < (int) _points.size(); i++)
            {
                if (_points[i].x() == p.x())
                {
                    throw std::exception("X-coordinates of original points must differ. ");
                }
            }

            _points.push_back(p);
            std::sort(_points.begin(), _points.end(), PointComparer);
        }

        /**
        * Moves values of the function along a given vector.
        * For example pass (0, 5) to add 5 to resulting values of the function
        */
        void Shift (gtl::Vec2<T> v)
        {
            for (int i = 0; i < (int) _points.size(); i++)
            {
                _points[i] += v;
            }
        }

        T operator () (T x) const
        {
            if (_points.size() < 2)
            {
                throw std::exception("Function cannot be fitted because too little original points are given. ");
            }
            double y = 0;
            if (x < _minX)
            {
                throw std::exception("x is less than min x. ");
            }
            if (x > _maxX)
            {
                throw std::exception("x is greater than max x. ");
            }


            gtl::Vec2<T> p1 = _points[0];
            gtl::Vec2<T> p2 = _points[1];
            for(int i = 1, n = _points.size() - 1; i < n; i++)
            {
                if (_points[i].x() > x)
                {
                    break;
                }
                p1 = _points[i];
                p2 = _points[i + 1];
            }

            assert(p2.x() != p1.x());
            y = (x - p1.x()) * (p2.y() - p1.y()) / (p2.x() - p1.x()) + p1.y();
            return y;
        }

        LinFitting & operator << (const gtl::Vec2<T> & pt)
        {
            AddPoint(pt);
            return *this;
        }

    private:
        std::vector<gtl::Vec2<T>> _points;
        T _minX, _maxX;

    };
    typedef LinFitting<double> LinFittingd;
    typedef LinFitting<float> LinFittingf;
}